#include <stdio.h>
#include <string.h>
#include "GPS_GSM_Tracker.h"
#include "system.h"

NavigationData navigationData;
ExtendedCellData cellData;

NavigationData lastGoodNavigationData;
ExtendedCellData lastGoodCellData;
PhonebookEntry myMaster;

uint32_t lastUpdateCellInfoTime = 0;
static const unsigned int updateCellInfoInterval = 10000;

//long lastUpdateGPSInfoTime;
//static const int updateGPSInfoInterval = 1000;

uint32_t lastAnswerMessagesTime = 1000;
static const unsigned int answerMessagesInterval = 10000;

uint32_t lastUpdateStatusIndicatorsTime = 0;
static const unsigned int updateStatusIndicatorsInterval = 5;

// uint32_t lastDebugTime = 0;
// static const unsigned int debugInterval = 1000;

uint16_t numValidGPSMessages;

void updateCellInfo() {
	if (gsm.getExtendedCellData(cellData)) {
		// cellData should be valid here so no check.
		if (lastGoodCellData.isValid && !lastGoodCellData.equals(&cellData)) {
			changedCellEvent();
		}
		lastGoodCellData = cellData;
	}
}

void changedCellEvent() {
	// Implement automatic send of SMS upon change of cell.
}

void updateGPSInfo() {
	if (gps.task() == NavigationData::NEWDATA) {
		// Check if this is a good position and adopt it if so.
		if (lastGoodNavigationData.isSupersededBy(&navigationData)) {
			lastGoodNavigationData = navigationData;
		}
		numValidGPSMessages++;
		toggleLED();
	}
}

size_t printTimestamp(char* text, uint32_t mtod) {
	int8_t hours = mtod / (60UL * 60UL * 1000UL);

	int16_t minutesRaw = mtod / (60UL * 1000UL);
	int8_t minutes = minutesRaw - hours * 60;

	int32_t secondsRaw = mtod / 1000UL;
	int8_t seconds = secondsRaw - minutesRaw * 60UL;

	return sprintf(text, "%02d:%02d:%02d", hours, minutes, seconds);
}

size_t makeTimestampMessage(char* text, NavigationData* navigationData) {
	size_t idx = sprintf(text, "GPS time: ");
	idx += printTimestamp(text+idx, navigationData->itod);
	// idx += sprintf(text+idx, " (uptime: ");
	// idx += printTimestamp(text+idx, getSystemTimeMillis());
	// idx += sprintf(text+idx, ")");
	return idx;
}

size_t makeGPSPositionMessage(char* text, NavigationData* navdata) {
	size_t pos = 0;
	pos += sprintf(text+pos, "Message # %d\n", numValidGPSMessages);
	pos += sprintf(text+pos, "Fix mode ");
	switch(navdata->fixMode) {
	case 0:
		pos += sprintf(text+pos, "NONE!");
		break;
	case 1:
		pos += sprintf(text+pos, "2D");
		break;
	case 2:
		pos += sprintf(text+pos, "3D");
		break;
	}

	pos += sprintf(text+pos, " (%d sats)\n", navdata->numberOfSatellites);

	double lat = navdata->position.latitude_d;
	char ns = lat >= 0 ? 'N' : 'S';
	if (lat < 0)
		lat = -lat;
	pos += sprintf(text+pos, "Lat %0.5f%c\n", lat, ns);

	double lon = navdata->position.longitude_d;
	char ew = lon >= 0 ? 'E' : 'W';
	if (lon < 0)
		lon = -lon;
	pos += sprintf(text + pos, "Lon %0.5f%c\n", lon, ew);

	pos += sprintf(text + pos, "Alt %0.0fm\n", navdata->position.altitude_f);

	pos += sprintf(text + pos, "Crs %0.0f",
					navdata->groundCourseSpeed.course_f);

	pos += sprintf(text + pos, ", Spd %0.1f",
			navdata->groundCourseSpeed.speed_f);

	return pos;
}

size_t makeExtendedCellDataMessage(char* text, ExtendedCellData* cellData) {
	return sprintf(text, "CellId %d\nMNC %d\nMCC %d\nLAC %d", cellData->cellId,
			cellData->MNC, cellData->MCC, cellData->LAC);
}

size_t generateAllDataMessage(char* text) {
	size_t pos;
	pos = makeTimestampMessage(text, &navigationData);
	pos += sprintf(text+pos, "\n");
	pos += makeGPSPositionMessage(text+pos, &navigationData);
	pos += sprintf(text+pos, "\n");
	pos += makeExtendedCellDataMessage(text+pos, &cellData);
	return pos;
}

void answerMessages() {
	SMS tempSMS;
	if (gsm.newReceivedSMS(tempSMS)) {
		size_t length = generateAllDataMessage(tempSMS.text);
		gsm.sendSMS(tempSMS); // number is in SMS already.
	}
}

void debug() {
	char lort[200];
	generateAllDataMessage(lort);
	printf("%s\n\n", lort);
}

/* This is implementation specific and is left to concrete subtypes.
 void updateStatusIndicators() {
 }
 */

void mainLoop() {
	while (true) {
		// Do very often. No reason not to.
		updateGPSInfo();

		uint32_t time = getSystemTimeMillis();

		if (time - lastAnswerMessagesTime > answerMessagesInterval) {
			answerMessages();
			lastAnswerMessagesTime = time;
		}

		if (time - lastUpdateCellInfoTime > updateCellInfoInterval) {
			updateCellInfo();
			lastUpdateCellInfoTime = time;
		}

		/*
		if (time - lastDebugTime > debugInterval) {
			debug();
			lastDebugTime = time;
		}
		*/
		if (time - lastUpdateStatusIndicatorsTime
				>= updateStatusIndicatorsInterval) {
			updateStatusIndicators();
			lastUpdateStatusIndicatorsTime = time;
		}

		sleep(10);
	}
}
